{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 无人机路径规划报告\n",
    "\n",
    "* 姓名：杨鹏\n",
    "* 学号：2018300468\n",
    "\n",
    "## 1. 任务简介\n",
    "**1.1背景概述**\n",
    "\n",
    "目前无人机虽然实现了自动飞行，但是大部分的无人机仍不具备感知环境、动态适应环境的能力。本课题通过一步一步实现无人机的感知、路径规划、仿真，通过本项目理解无人机系统的感知与控制。\n",
    "\n",
    "\n",
    "**1.2任务**:\n",
    "\n",
    "* 首先根据所给地图文件，绘制出地图网格，并用不同颜色标志区别起点，终点和障碍\n",
    "\n",
    "* 其次学习并实现A * 算法，在这个阶段可以假设环境地图都已知，规划全局的地图，并显示在GUI界面上\n",
    "\n",
    "* 模拟激光雷达扫描。模拟从当前位置发射多条射线，判断障碍物是否在射线上。把结果显示在在GUI界面上\n",
    "\n",
    "* 设计动态路径的算法，并实现该算法。测试所实现的方法和示例效果的差别，并改进\n",
    "\n",
    "* 功能实现后进行代码重构\n",
    "\n",
    "**1.3文件和数据说明**:\n",
    "\n",
    "|数据文件名|格式|功用|\n",
    " |-|-|-|\n",
    " |test_1.map   |map|地图数据集1，包含地图尺寸，起点终点位置，障碍位置|\n",
    " |test_7.map   |map|地图数据集7，包含地图尺寸，起点终点位置，障碍位置|\n",
    "\n",
    "\n",
    "信息名|格式|内容\n",
    ":--:|:--:|:--:\n",
    "f_mag|uint32_t|文件标识，内容是：0x15432345\n",
    "MAP.mx|int|map的宽度-mx，像素数\n",
    "MAP.my|int|map的长度-my，像素数\n",
    "startX|int|起点的X坐标\n",
    "startY|int|起点的Y坐标\n",
    "endX|int|终点的X坐标\n",
    "endY|int|终点的Y坐标\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 2. 报告内容简述\n",
    "1.本次报告主要分为三个部分。第一个部分是实现地图文件的读取，网格地图的动态可视化。\n",
    "\n",
    "第二个部分是在地图障碍已知的情况下，学习并实现A* 算法规划路径，最后动态展示。\n",
    "\n",
    "第三个部分是在地图未知的情况下实现雷达扫掠模拟探测路径，并进行动态的路径规划，并动态展示。\n",
    "\n",
    "2.本次课设使用matlab编程，matlab可以简单实现网格的可视化，并且相对于c语言，在矩阵运算上方便快捷，也有一些方便使用的函数，也可以简单的将自己写好的程序封装为函数保存，因此使用matlab进行编程。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 3.实现过程\n",
    "\n",
    "### 3.1.1 地图文件读取\n",
    ".map文件为二进制文件，其中包含了地图的各个信息。\n",
    "\n",
    "使用matlab中的文件打开函数fopen打开文件，并使用文件指针，根据文件格式判断文件打开是否正确，若打开错误则会有错误提示。之后按顺序提取地图信息，如地图尺寸，地图起点终点的坐标和地图“全貌”，注意这样打开后，所有的数据都需要转置，这样最后可视化的时候图片才和课设首页的正向一样。\n",
    "\n",
    "地图读取程序如下："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "ename": "SyntaxError",
     "evalue": "invalid syntax (<ipython-input-1-a2613d13ba2e>, line 1)",
     "output_type": "error",
     "traceback": [
      "\u001b[1;36m  File \u001b[1;32m\"<ipython-input-1-a2613d13ba2e>\"\u001b[1;36m, line \u001b[1;32m1\u001b[0m\n\u001b[1;33m    function MAP=read_map(file_address)\u001b[0m\n\u001b[1;37m               ^\u001b[0m\n\u001b[1;31mSyntaxError\u001b[0m\u001b[1;31m:\u001b[0m invalid syntax\n"
     ]
    }
   ],
   "source": [
    "function MAP=read_map(file_address)\n",
    "    %%\n",
    "    %读取地图文件\n",
    "    %file_address='D:\\Git\\aircraft-information-system-design\\project-uav-planning\\maps\\test_1.map';  %文件地址\n",
    "    fp = fopen(file_address);\n",
    "    if( fp == -1 )\n",
    "        disp(\"ERR: failed to open file \" + file_address + newline);\n",
    "        return;\n",
    "    end\n",
    "    f_mag = fread(fp, 1, 'uint32');\n",
    "    if( f_mag ~= hex2dec('15432345') )     %十六进制转二进制判断\n",
    "        disp(\"ERR: input file format is not correct! \" + file_address + newline);\n",
    "        fclose(fp);\n",
    "        return;\n",
    "    end\n",
    "    MAP.my = fread(fp, 1, 'int32');   %读取地图尺寸\n",
    "    MAP.mx = fread(fp, 1, 'int32');\n",
    "    % read map\n",
    "    MAP.map = fread(fp, [MAP.mx MAP.my], 'int8');\n",
    "    MAP.map = MAP.map';   %数据需要转置\n",
    "    %读障碍坐标\n",
    "    n=1;\n",
    "    for i=1:MAP.my\n",
    "        for j=1:MAP.mx\n",
    "            if MAP.map(i,j)==1\n",
    "                ob(1,n)=i; ob(2,n)=j;\n",
    "                n=n+1;\n",
    "            end\n",
    "        end\n",
    "    end\n",
    "    % 读起始点和终止点坐标\n",
    "    MAP.startY = fread(fp, 1, 'int32');\n",
    "    MAP.startX = fread(fp, 1, 'int32');\n",
    "    % MAP.startY =40;\n",
    "    % MAP.startX =70;\n",
    "    MAP.nowx=MAP.startX;   %现在点坐标\n",
    "    MAP.nowy=MAP.startY;\n",
    "    MAP.endY = fread(fp, 1, 'int32');\n",
    "    MAP.endX = fread(fp, 1, 'int32');\n",
    "    fclose(fp);\n",
    "end"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.1.2 网格可视化实现\n",
    "matlab使用句柄操作可以简单实现网格可视化，输入一个二维数组，赋值0或1，既可显示一个简单的界面,设置句柄方便后续在这个图的基础上修改。\n",
    "\n",
    "通过第一步读取.map文件，把map读取到一个二维数组中，就能画出该地图\n",
    "\n",
    "代码如下："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%%\n",
    "%----------------网格初始化-------------------------\n",
    "set(gcf,'DoubleBuffer','on');\n",
    "S=MAP.map;\n",
    "S(MAP.startX,MAP.startY)=1;\n",
    "S(MAP.endX,MAP.endY)=2;\n",
    "Ii=imshow(cat(3,(S==1),(S==2),MAP.map));\n",
    "axis square;"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "结果如下图所示：\n",
    "\n",
    "红色表示起点，绿色表示终点，粉色为边界和障碍，其中起点的赋值为1，终点的赋值为2，障碍的赋值为1，其他黑色部分的值为0。\n",
    "\n",
    "<img src=\"./图片/地图1.png\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src=\"./图片/地图2.png\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.2.1 A* 算法实现路径规划\n",
    "A* 算法关键词：最短路径，当前代价，预估代价\n",
    "\n",
    "经过一段时间学习，我简单概括一下A* 算法内容与过程：\n",
    "\n",
    "A* 算法用于路径规划，其特点在于使用“代价”来评判选择路径优先程度。过程为：首先确定起点和终点，从起点开始，以起点四周或周围八格开始选择路径，计算给一个方格的当前代价和预估代价，当前代价为从起点到该格要走的距离，预估代价为该格到终点的距离。计算周围格子的当前代价和预估代价之和，并比较该和值，选择和最小的方格作为最优先的路径，并重复该操作，直到能找到终点位置。\n",
    "\n",
    "简述一下步骤：\n",
    "\n",
    "**1** 首先将设置开启列表和关闭列表，开启列表为探索边界，每次探索从这些点开始，关闭列表为已经不需要探索或者为障碍的点。先将起点放入开启列表中，寻找其上下左右四方向的点，将他们也放入开启列表中，并将起点从开启列表中删去，放入关闭列表。\n",
    "\n",
    "**2** 计算四方向点的F值，设置好其父节点，保存好。从开启列表中找到F值最小的格子，将其从开启列表移到关闭列表。找到这个点的四方向点，依次判断这些点的情况。情况1：点在关闭列表内或者为障碍，则不操作；情况2，点不在开启列表，将它加入到开启列表，计算F值，设置父节点。情况3，在开启列表内，计算其当前的G，G_now等于这个点到起点的距离，并与原先加入开启列表时的G进行比较，若G_now小于G_past，替换G，替换父节点。重复以上操作，直到终点被包进开启列表则寻路结束，退出循环。\n",
    "\n",
    "**3** 从终点开始，每次加入开启节点都要设置其父节点，因此从终点开始寻找父节点，依次迭代，就能找到一条从终点到起点的路径，这就是A* 算法最终寻路的路径，显然，该路径不唯一。\n",
    "\n",
    "tip：本次A* 算法实现，我选择了周围四格，即无人机前进方向只有上下左右四个方向，计算代价值采用曼哈顿距离，避免开方运算\n",
    "\n",
    "以下为代码实现："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "ename": "SyntaxError",
     "evalue": "invalid syntax (<ipython-input-2-f3a7ef9fb6c3>, line 1)",
     "output_type": "error",
     "traceback": [
      "\u001b[1;36m  File \u001b[1;32m\"<ipython-input-2-f3a7ef9fb6c3>\"\u001b[1;36m, line \u001b[1;32m1\u001b[0m\n\u001b[1;33m    function [open_list close_list S_scan]=Asta(S,MAP,Ii)\u001b[0m\n\u001b[1;37m                                 ^\u001b[0m\n\u001b[1;31mSyntaxError\u001b[0m\u001b[1;31m:\u001b[0m invalid syntax\n"
     ]
    }
   ],
   "source": [
    "function [open_list close_list S_scan]=Asta(S,MAP,Ii)\n",
    "    %%\n",
    "    %------------------实现A*算法------------------\n",
    "    %下方向为X正向，向右为y正向\n",
    "    %第一步，将起点放入开启列表，寻找起点四周的四个点，将其放入开启列表，然后从开启列表中删除起点，放入关闭列表\n",
    "    open_list=[MAP.nowx ;MAP.nowy;0];\n",
    "    close_list=[];\n",
    "    close_list(1,end+1)=open_list(1,1); close_list(2,end)=open_list(2,1);\n",
    "\n",
    "    open_list(1,end+1)=open_list(1,1)-1; open_list(2,end)=open_list(2,1);\n",
    "    open_list(1,end+1)=open_list(1,1)+1; open_list(2,end)=open_list(2,1);\n",
    "    open_list(1,end+1)=open_list(1,1); open_list(2,end)=open_list(2,1)+1;\n",
    "    open_list(1,end+1)=open_list(1,1); open_list(2,end)=open_list(2,1)-1;\n",
    "    open_list(:,1)=[];\n",
    "    % open_list\n",
    "    % close_list\n",
    "\n",
    "    %第二步，计算每个附近点的F值，将其放在open_list中\n",
    "    %open_list第一行为x坐标，第二行为y坐标，三四五为H,G,F，六七行为父节点xy坐标。\n",
    "    for i=1:length(open_list(1,:))\n",
    "        %%------------计算预估距离------------------\n",
    "        %%-----------采用曼哈顿距离-----------\n",
    "        if MAP.map(open_list(1,i),open_list(2,i))==1 \n",
    "            MAP.map(open_list(1,i),open_list(2,i))==1 ;   \n",
    "            continue;\n",
    "        end\n",
    "        H=abs(open_list(1,i)-MAP.endX)+abs(open_list(2,i)-MAP.endY);  \n",
    "        open_list(3,i)=H;\n",
    "        open_list(4,i)=1;\n",
    "        open_list(5,i)=open_list(4,i)+open_list(3,i);\n",
    "        open_list(6,i)=close_list(1,1);\n",
    "        open_list(7,i)=close_list(2,1);\n",
    "    end\n",
    "    [~,loc]=find(open_list(3,:)==0 & open_list(4,:)==0 & open_list(5,:)==0 & open_list(6,:)==0 & open_list(7,:)==0);\n",
    "    open_list(:,loc)=[];\n",
    "    % open_list;\n",
    "\n",
    "    %第三步，开始循环\n",
    "    while( 1)\n",
    "        %从开启列表中找到F值最小的格子，将其从开启列表中删除，并放进关闭列表中\n",
    "        [Fmin ind]=sort(open_list(5,:));\n",
    "        close_list(1,end+1)=open_list(1,ind(1)); close_list(2,end)=open_list(2,ind(1));\n",
    "        %三 四行存放父节点\n",
    "        close_list(3,end)=open_list(6,ind(1)); close_list(4,end)=open_list(7,ind(1));\n",
    "        open_list(:,ind(1))=[];\n",
    "    %     open_list\n",
    "    %     close_list\n",
    "\n",
    "        rp_list=[close_list(1,end)-1 close_list(1,end)+1 close_list(1,end) close_list(1,end);close_list(2,end) close_list(2,end) close_list(2,end)+1 close_list(2,end)-1];\n",
    "        %rp_list存放每个判断点四周的格子的xy坐标\n",
    "        S(close_list(1,end),close_list(2,end))=1;\n",
    "        set(Ii,'CData',cat(3,(S==1),(S==2),MAP.map) );\n",
    "        S(close_list(1,end),close_list(2,end))=0;\n",
    "\n",
    "        %循环判断每一个附近的格子\n",
    "        for i=1:length(rp_list(1,:))\n",
    "            %如果附近的格子在关闭列表内或者是障碍，则不操作\n",
    "            if MAP.map(rp_list(1,i),rp_list(2,i))==1 | sum(ismember(close_list(1:2,:)',[rp_list(1,i) rp_list(2,i)],'row' ))==1\n",
    "                %MAP.map(rp_list(1,i),rp_list(2,i))==1 | sum(ismember(close_list(1:2,:)',[rp_list(1,i) rp_list(2,i)],'row' ))==1\n",
    "                continue;    \n",
    "            else \n",
    "                %否者，如果方格不在开启列表中，将其加入开启列表，并计算F值，设置父节点\n",
    "                if sum(ismember(open_list(1:2,:)',[rp_list(1,i) rp_list(2,i)],'row' ))==0\n",
    "                    open_list(1,end+1)=rp_list(1,i); open_list(2,end)=rp_list(2,i);\n",
    "                    H=abs(rp_list(1,i)-MAP.endX)+abs(rp_list(2,i)-MAP.endY);\n",
    "                    open_list(3,end)=H;\n",
    "                    open_list(4,end)=abs(rp_list(1,i)-MAP.nowx)+abs(rp_list(2,i)-MAP.nowy);\n",
    "                    open_list(5,end)=open_list(4,end)+open_list(3,end);\n",
    "                    open_list(6,end)=close_list(1,end);\n",
    "                    open_list(7,end)=close_list(2,end);\n",
    "\n",
    "                    %绘制边界点\n",
    "                    S(rp_list(1,i),rp_list(2,i))=2;\n",
    "                    set(Ii,'CData',cat(3,(S==1),(S==2),MAP.map) );\n",
    "                    pause(0.001);\n",
    "\n",
    "                    %open_list  %查看open_list\n",
    "                else\n",
    "                    %如果在，计算从起点到该点的距离，即为当前的G，与原先的G比较\n",
    "                    %若小于，替换小的G，重新计算F，替换父节点\n",
    "                    j=find(ismember(open_list(1:2,:)',[rp_list(1,i) rp_list(2,i)],'row' )==1);\n",
    "                    G1=abs(rp_list(1,i)-MAP.nowx)+abs(rp_list(2,i)-MAP.nowy);\n",
    "                    G2=open_list(4,j);\n",
    "                    if G1<G2\n",
    "                        open_list(4,j)=G1;\n",
    "                        open_list(5,j)=open_list(4,j)+open_list(3,j);\n",
    "                        open_list(6,j)=rp_list(1,i);\n",
    "                        open_list(7,j)=rp_list(2,i);\n",
    "                    end\n",
    "                end    \n",
    "            end\n",
    "        end\n",
    "        %当终点被包含在开启列表中时，代表能寻路，此时终止寻路\n",
    "        if sum(ismember(open_list(1:2,:)',[MAP.endX,MAP.endY],'row' ))==1\n",
    "            disp('寻路完毕');\n",
    "            break;\n",
    "        end    \n",
    "    end\n",
    "    S_scan=S;\n",
    "end"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "以地图1为例，结果如下图，其中绿色为探索边界，即为开启列表中的各个方格，当边界探索到终点时，寻路结束\n",
    "\n",
    "以下为实现动态过程：\n",
    "\n",
    "<img src=\"./图片/GIF1.gif\">\n",
    "\n",
    "<img src=\"./图片/GIF5.gif\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.2.2 路径可视化\n",
    "在每一次计算后我都把对应方格的父方格更新在开启列表和关闭列表中，因此只要从开启列表中找到终点，再找到其对应的父节点，再根据父节点在关闭列表中找到下一个节点，并由此循环直至找到起点，将各个点标出，即为本次寻路的最短路径。\n",
    "\n",
    "最后将父节点倒叙到处，就成为了从起点到终点的路径。\n",
    "\n",
    "路径可视化程序实现如下："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "ename": "SyntaxError",
     "evalue": "invalid syntax (<ipython-input-3-cdf4ec721fa2>, line 1)",
     "output_type": "error",
     "traceback": [
      "\u001b[1;36m  File \u001b[1;32m\"<ipython-input-3-cdf4ec721fa2>\"\u001b[1;36m, line \u001b[1;32m1\u001b[0m\n\u001b[1;33m    function road_list=road_print(S,MAP,Ii,open_list,close_list)\u001b[0m\n\u001b[1;37m                     ^\u001b[0m\n\u001b[1;31mSyntaxError\u001b[0m\u001b[1;31m:\u001b[0m invalid syntax\n"
     ]
    }
   ],
   "source": [
    "function road_list=road_print(S,MAP,Ii,open_list,close_list)\n",
    "    %%\n",
    "    disp('绘制路径中。。。。')\n",
    "    %绘制路径，根据每次放入关闭列表中，方格对应的父节点，从终点开始一次寻找父节点，既能绘制出路径\n",
    "    road_list=[];  %创建节点列表，即为路径列表\n",
    "    ind_fin=find(ismember(open_list(1:2,:)',[MAP.endX MAP.endY ],'row')==1);\n",
    "    S(open_list(6,ind_fin),open_list(7,ind_fin))=1;\n",
    "    road_list(1,1)=open_list(6,ind_fin);  road_list(2,1)=open_list(7,ind_fin);  %载入第一个节点 \n",
    "    set(Ii,'CData',cat(3,(S==1),(S==2),MAP.map) );\n",
    "    pause(0.1);\n",
    "    ind_next=find(ismember(close_list(1:2,:)',[open_list(6,ind_fin) open_list(7,ind_fin)],'row')==1);\n",
    "    % close_list(3,ind_next)\n",
    "    % close_list(4,ind_next)\n",
    "    S(close_list(3,ind_next),close_list(4,ind_next))=1;\n",
    "    road_list(1,end+1)=close_list(3,ind_next);  road_list(2,end)=close_list(4,ind_next);  %载入第二个节点 \n",
    "    set(Ii,'CData',cat(3,(S==1),(S==2),MAP.map) );\n",
    "    pause(0.1);\n",
    "    while (1)\n",
    "        ind_next=find(ismember(close_list(1:2,:)',[close_list(3,ind_next) close_list(4,ind_next)],'row')==1);\n",
    "    %     close_list(3,ind_next)\n",
    "    %     close_list(4,ind_next)\n",
    "        if ind_next==1\n",
    "            disp('路径绘制完毕');\n",
    "            break;\n",
    "        end\n",
    "        %绘制\n",
    "        S(close_list(3,ind_next),close_list(4,ind_next))=1;\n",
    "        road_list(1,end+1)=close_list(3,ind_next);  road_list(2,end)=close_list(4,ind_next);  %载入第二个节点 \n",
    "        set(Ii,'CData',cat(3,(S==1),(S==2),MAP.map) );\n",
    "        pause(0.01);\n",
    "    end\n",
    "end"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "路径动态过程如下：\n",
    "\n",
    "<img src=\"./图片/GIF2.gif\">\n",
    "\n",
    "<img src=\"./图片/GIF6.gif\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "本次实现规定无人机只能按照上下左右四个方向运动，因此无法做到“走斜线”，但很明显，有些情况下走斜线才是最短的。不过因为无人机在模拟的情况下每次只走一格，所以一格为单位路径，走斜线和走直线效果实际是一样的，所以可视这种情况为最短路径。\n",
    "\n",
    "至此，第一步和第二步基本实现"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.3.1 实现模拟雷达扫掠\n",
    "\n",
    "本问题的背景：\n",
    "\n",
    "首先，与第一问不同，本次无人机是在地图未知，起始点和终点已知的情况下先进行雷达扫掠，先探测整个地图，在开始时默认地图无障碍，进行A* 路径搜索，然后让有雷达搜索功能的无人机按照这个路径前进，并探索，把障碍记录下来。当无人机遇到障碍时，就根据已知的障碍再重新规划路径，并重复此操作，直到找到终点为止。\n",
    "\n",
    "雷达功能实现：\n",
    "\n",
    "1.假设以无人机为圆心，半径为r的圆为雷达搜索范围，搜索地图内为障碍的点\n",
    "\n",
    "2.由第一步找到的障碍点会有“过界”，因此还需要对障碍点进行筛选，找到没有过界的点\n",
    "\n",
    "做法：循环遍历地图每个点，计算到无人机当前位置的距离，如果在半径范围内，并且点对应的值为1（即为障碍和墙壁），则认为雷达判断出地形了，记录下来。至于筛选过界的点，现在我还没有想到一个较好的方法，每次的探索都会有一些过界的点，这个地方还需要继续完善，下面展示的结果都没有很好的解决这个问题。不过我实现的探索基本都能把障碍的边界给探索出来，因此基本不会影响到最终的路径规划。\n",
    "\n",
    "雷达探测实现程序如下："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "ename": "SyntaxError",
     "evalue": "invalid syntax (<ipython-input-5-109de95284bb>, line 1)",
     "output_type": "error",
     "traceback": [
      "\u001b[1;36m  File \u001b[1;32m\"<ipython-input-5-109de95284bb>\"\u001b[1;36m, line \u001b[1;32m1\u001b[0m\n\u001b[1;33m    function [S3 ob_real]=scan_ob(MAP,road_x,road_y,R)\u001b[0m\n\u001b[1;37m                       ^\u001b[0m\n\u001b[1;31mSyntaxError\u001b[0m\u001b[1;31m:\u001b[0m invalid syntax\n"
     ]
    }
   ],
   "source": [
    "function [S3 ob_real]=scan_ob(MAP,road_x,road_y,R)\n",
    "%搜索障碍点\n",
    "        S3=zeros(100);\n",
    "        S3(MAP.endX,MAP.endY)=2;\n",
    "        S3(road_x,road_y)=1;\n",
    "        ob_list=[0;0];\n",
    "        ob_real=[0;0];\n",
    "        for i=1:100   %首先根据雷达扫掠半径找到所有在半径内的障碍点，放在ob_list\n",
    "            for j=1:100\n",
    "                if abs(i-road_x)^2+abs(j-road_y)^2 < R^2 & abs(i-road_x)^2+abs(j-road_y)^2 ~=0 %判断是否在检测范围内\n",
    "                    abs(i-road_x)^2+abs(j-road_y)^2;\n",
    "                    if MAP.map(i,j)==1\n",
    "                        ob_list(1,end+1)=i;  ob_list(2,end)=j;\n",
    "                    end\n",
    "                end\n",
    "            end\n",
    "        end\n",
    "        ob_list(:,1)=[];\n",
    "        ob_x=[0:0];  ob_y=[0:0];\n",
    "\n",
    "        %去掉“过界”障碍点，原理是以无人机为中心出发，向各个方向的障碍点数只为1，将其分为x,y两个方向，即ob_x和ob_y\n",
    "        for i=1:length(ob_list(1,:))    \n",
    "            if ob_list(2,i)<road_y\n",
    "                p1=MAP.map(ob_list(1,i),ob_list(2,i):road_y);\n",
    "                if sum(p1)==1\n",
    "                    ob_x(1,end+1)=ob_list(1,i); ob_x(2,end)=ob_list(2,i);\n",
    "                end\n",
    "            elseif ob_list(2,i) > road_y\n",
    "                p1=MAP.map(ob_list(1,i),road_y:ob_list(2,i));\n",
    "                if sum(p1)==1 \n",
    "                   ob_x(1,end+1)=ob_list(1,i); ob_x(2,end)=ob_list(2,i);\n",
    "                end \n",
    "            elseif ob_list(2,i) == road_y\n",
    "                if ob_list(1,i)>road_x\n",
    "                    p1=MAP.map(road_x:ob_list(1,i),ob_list(2,i));\n",
    "                else \n",
    "                    p1=MAP.map(ob_list(1,i):road_x,ob_list(2,i));\n",
    "                end\n",
    "                if sum(p1)==1 \n",
    "                   ob_x(1,end+1)=ob_list(1,i); ob_x(2,end)=ob_list(2,i);\n",
    "                end \n",
    "            end\n",
    "\n",
    "            if ob_list(1,i)<road_x\n",
    "                p1=MAP.map(ob_list(1,i):road_x,ob_list(2,i));\n",
    "                if sum(p1)>1\n",
    "                    ob_y(1,end+1)=ob_list(1,i); ob_y(2,end)=ob_list(2,i);\n",
    "                end\n",
    "            elseif ob_list(1,i) > road_x\n",
    "                p1=MAP.map(road_x:ob_list(1,i),ob_list(2,i));\n",
    "                if sum(p1)==1 \n",
    "                    ob_y(1,end+1)=ob_list(1,i); ob_y(2,end)=ob_list(2,i);\n",
    "                end\n",
    "            elseif ob_list(1,i) == road_x\n",
    "                if ob_list(2,i)>road_y\n",
    "                    p1=MAP.map(ob_list(1,i),road_y:ob_list(2,i));\n",
    "                elseif ob_list(2,i) < road_y\n",
    "                    p1=MAP.map(ob_list(1,i),ob_list(2,i):road_y);\n",
    "                end\n",
    "                if sum(p1)==1 \n",
    "                   ob_y(1,end+1)=ob_list(1,i); ob_y(2,end)=ob_list(2,i);\n",
    "                end \n",
    "            end\n",
    "        end\n",
    "        %ob_real存放真正的障碍点\n",
    "        ob_x(:,1)=[]; ob_y(:,1)=[];\n",
    "        ob_real=[ob_x,ob_y];\n",
    "%       ob_real\n",
    "end"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "雷达扫掠过程如下所示\n",
    "\n",
    "<img src=\"./图片/GIF3.gif\">\n",
    "\n",
    "<img src=\"./图片/GIF4.gif\">"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "上面三个图，第一个图为假设没有障碍的情况下，使用A* 算法探索路径\n",
    "\n",
    "第二个图为无人机沿着第一个图探索的路径前进，边前进边扫掠，当遇到障碍时停止\n",
    "\n",
    "第三个图为无人机内部记录的扫描后的地图全貌，这个图会不断更新，作为A* 算法迭代的地图基础"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.3.2 路径规划\n",
    "\n",
    "个人想法：在第一步雷达扫掠的基础上，实现先规划，边移动边扫掠，遇到障碍后根据已经记录下来的障碍再重新规划路径，再移动扫掠，不断更新地图，重复以上操作，直到无人机到达终点为止。\n",
    "\n",
    "动态规划过程：\n",
    "\n",
    "**1** 首先将无人机放于一个未知的环境中，已知无人机的起点和终点，先进行A* 路径规划，无人机按照算法规划路径前进，直到接触到障碍后停止\n",
    "\n",
    "**2** 之后无人机根据已经扫掠的地图为基础，再进行A* 算法规划，无人机再按这个路径前进\n",
    "\n",
    "**3** 循环以上操作，直到无人机到终点后结束循环\n",
    "\n",
    "以下为代码实现："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "UsageError: Cell magic `%%` not found.\n"
     ]
    }
   ],
   "source": [
    "%%\n",
    "%第一步，读取地图\n",
    "clear all;\n",
    "MAP=load_map('D:\\Git\\aircraft-information-system-design\\project-uav-planning\\maps\\test_1.map');"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "首先读取地图，一些步骤我已经将其放入函数中，所以直接调用即可，函数我也会上传，地图地址为自己的地图所在文件夹地址"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "UsageError: Cell magic `%%` not found.\n"
     ]
    }
   ],
   "source": [
    "%%\n",
    "%第二步，首先假设地图障碍未知，使用A*算法寻路\n",
    "MAP2=MAP;\n",
    "MAP2.map(2:99,2:99)=0;  %默认地图无障碍\n",
    "set(gcf,'DoubleBuffer','on');\n",
    "S=MAP2.map;\n",
    "S(MAP2.startX,MAP2.startY)=1;\n",
    "S(MAP2.endX,MAP2.endY)=2;\n",
    "subplot(2,2,1); title('A*寻路动态图');\n",
    "I1=imshow(cat(3,(S==1),(S==2),MAP2.map));\n",
    "axis square;"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "初始化网格地图"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "UsageError: Cell magic `%%` not found.\n"
     ]
    }
   ],
   "source": [
    "%%\n",
    "%使用A*寻路\n",
    "road_real=[];  %存放最终的路径\n",
    "[open_list2 close_list2 S_scan]=Asta(S,MAP2,I1);\n",
    "road_list=road_print(S_scan,MAP2,I1,open_list2,close_list2);\n",
    "road_list=fliplr(road_list);\n",
    "road_list(1,end+1)=MAP2.endX;  road_list(2,end)=MAP2.endY;"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "在地图未知的情况下，进行第一次算法路径规划，road_real为最后路径节点保存列表"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "UsageError: Cell magic `%%` not found.\n"
     ]
    }
   ],
   "source": [
    "%%\n",
    "%第三步，模拟雷达探索路径\n",
    "R=30;  %设雷达探测的半径为30格\n",
    "subplot(2,2,2);  title('雷达扫掠图');\n",
    "S2=MAP.map;\n",
    "set(gcf,'DoubleBuffer','on');\n",
    "S2(road_list(1,1),road_list(2,1))=1;\n",
    "S2(MAP.endX,MAP.endY)=2;\n",
    "I2=imshow(cat(3,(S2==1),(S2==2),MAP.map));\n",
    "axis square;\n",
    "\n",
    "S4=zeros(100);\n",
    "subplot(2,2,3);  title('无人机障碍记录图');\n",
    "I3=imshow(cat(3,(S4==1),(S4==2),S4));\n",
    "axis square;\n",
    "\n",
    "while 1\n",
    "    %搜索障碍点\n",
    "    for k=1:length(road_list(1,:))\n",
    "        [S3 ob_real]=scan_ob(MAP,road_list(1,k),road_list(2,k),R);\n",
    "        %动态更新图像\n",
    "        for i=1:length(ob_real(1,:))\n",
    "            S3(ob_real(1,i),ob_real(2,i))=2;\n",
    "            set(I2,'CData',cat(3,(S3==1),(S3==2),MAP.map) );\n",
    "            S4(ob_real(1,i),ob_real(2,i))=1;\n",
    "            set(I3,'CData',cat(3,(S4==1),(S4==2),S4) );\n",
    "        end\n",
    "        pause(0.01);\n",
    "        if sum([road_list(1,k),road_list(2,k)]==[MAP.endX ,MAP.endY])==2\n",
    "            road_real=[road_real,road_list(1:2,1:k)];\n",
    "            break;\n",
    "        end\n",
    "        if MAP.map(road_list(1,k+1),road_list(2,k+1))==1 \n",
    "            MAP2.nowx=road_list(1,k); MAP2.nowy=road_list(2,k);\n",
    "            road_real=[road_real,road_list(:,1:k)];\n",
    "            break;\n",
    "        end\n",
    "    end\n",
    "    %%\n",
    "    %第四步，判断是否到达终点\n",
    "    if road_real(1,end)==MAP.endX &  road_real(2,end)==MAP.endY\n",
    "        disp('雷达探索寻路实现完毕！')\n",
    "        break;\n",
    "    end\n",
    "    %%\n",
    "    %第五步，当遇到障碍时，重新规划路径，设此时为新的起点,此时以及探索出一些障碍，更新地图\n",
    "%     MAP2.nowx\n",
    "%     MAP2.nowy\n",
    "    MAP2.map=S4;\n",
    "    S4(MAP2.nowx,MAP.nowy)=1;\n",
    "    S4(MAP2.endX,MAP.endY)=2;\n",
    "    %  使用A*寻路\n",
    "    subplot(2,2,1);\n",
    "    I1=imshow(cat(3,(S4==1),(S4==2),S4));\n",
    "    [open_list2 close_list2 S4_scan]=Asta(S4,MAP2,I1);\n",
    "    road_list=road_print(S4_scan,MAP2,I1,open_list2,close_list2);\n",
    "    road_list=fliplr(road_list);\n",
    "    road_list(1,end+1)=MAP2.endX;  road_list(2,end)=MAP2.endY;\n",
    "    %%\n",
    "    %第六步，开始循环第三，四，五步，直到能找到一条没有障碍到终点的路径\n",
    "    pause(1);\n",
    "end"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "开始循环扫掠障碍和动态显示，部分步骤为使用之前封装好的函数"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "UsageError: Cell magic `%%` not found.\n"
     ]
    }
   ],
   "source": [
    "%%\n",
    "%绘制最终的路径图\n",
    "subplot(2,2,4);  title('雷达探测的最终实际路径图');\n",
    "set(gcf,'DoubleBuffer','on');\n",
    "S=MAP.map;\n",
    "S(MAP.startX,MAP.startY)=1;\n",
    "S(MAP.endX,MAP.endY)=2;\n",
    "I4=imshow(cat(3,(S==1),(S==2),MAP.map));\n",
    "axis square;\n",
    "for i=1:length(road_real(1,:))\n",
    "    S(road_real(1,i),road_real(2,i))=1;\n",
    "    set(I4,'CData',cat(3,(S==1),(S==2),MAP.map) );\n",
    "    pause(0.05);\n",
    "end"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "最后的可视化，将无人机最终的路径显示出来\n",
    "\n",
    "最终结果如下图所示：\n",
    "\n",
    "<img src=\"./图片/GIF7.gif\">\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "本次可视化分为四个图\n",
    "\n",
    "第一个图可理解为无人机内部A* 算法计算过程的动态图，每一次迭代都会更新这个图，无人机会根据雷达扫掠的地形来重新规划路径\n",
    "\n",
    "第二个图为雷达扫掠范围图\n",
    "\n",
    "第三个图为雷达扫掠后无人机记录下的地形图\n",
    "\n",
    "第四个图为最终无人机实际走了路径图\n",
    "\n",
    "这四个图每次计算都在实时更新\n",
    "\n",
    "下图为使用地图7后的路径规划：\n",
    "\n",
    "<img src=\"./图片/GIF8.gif\">\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "从这个例子可以明显看到之前提到关于障碍点“过界”优化时，我并没有做好处理，但是还是能够把障碍的主要外部轮廓探索出来，因此其实对于路径的规划没有太大影响，只是不符合实际情况，之后这块还有待思考，看是否需要其他一些方法来解决。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "至此，无人机雷达扫掠和路径规划基本实现，但还存在一些问题待解决\n",
    "\n",
    "与老师主页的结果图做对比：\n",
    "\n",
    "首先第一个不同是老师主页的路径可以实现走斜线，这里我想可能是因为在实现A* 算法的时候规定的走向不同，若规定八向前进，应该也能实现走斜线，所以这一点可以通过修改代码实现。\n",
    "\n",
    "其次，第二个不同点是雷达探索的探索点不同，这一部分需要继续优化编写的障碍扫掠程序。\n",
    "\n",
    "然后是可视化的不同，为了让自己在实现过程中的思路更清晰，我在可视化的时候尽量把每一步做的过程都呈现出来，这样我才能更好的判断到哪一步了，那一步有问题，有什么样的问题，有助于我的debug。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3.4 代码重构\n",
    "\n",
    "在初步编写代码后我已经将一部分代码封装为函数，模块化了，并同步更新在报告里，和提交的代码里面。目前代码中还有部分是为了后续方便调试而加的代码，若只是要实现功能，这部分代码可以删去。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4. 深入思考\n",
    "\n",
    "* 如何实现无人机的定位？\n",
    "\n",
    "初步想法，假若将无人机放于一个完全未知的地图，但目标终点与无人机位置的相对情况已知，则可以以目标点为极点构建极坐标系，实现无人机定位。当然若无人机带有GPS模块也能轻松实现定位。\n",
    "\n",
    "* 如何使用三维激光雷达生成三维地图？\n",
    "\n",
    "想法：三点既可确定一个平面，通过三维激光雷达扫掠，雷达发射出的三道激光在物体上投射的三个点构成一个平面，随着雷达不断深入扫掠，可以类比于微积分的方式对立体图像进行“积分”，将一个个平面积起来就能构建一个立体的三维图像。\n",
    "\n",
    "* 如何使用相机生成三维地图？\n",
    "\n",
    "三维全景地图是由专业的街景采集车经过图片采集和处理后实现的，基本原理是将周围720度的照片全部进行拍摄，在进行拼接等处理，最后生成三维全景图。\n",
    "\n",
    "* 如何考虑飞行器的飞行特性，实现平滑的轨迹？\n",
    "\n",
    "首先如果想展现出飞行器真实的轨迹，使用一个网格代表一个无人机，一格是无人机前进的单位速度这种假设肯定是不行的，首先运行地图需要更加精细，其次需要将无人机的运动轨迹给解算出来，包括无人机的单位速度，转弯半径，无人机大小等数据。其次，在考虑无人机运动时要考虑多向运动，根据地图和无人机的尺寸确定最小转弯角度等。\n",
    "\n",
    "* 如果是多个无人机，如何共享地图，加快搜索和任务执行效率？\n",
    "\n",
    "个人想法，多个无人机应该有同一个终端，都无人机可以访问这个终端，地图数据存放于这个终端，并且可以可以给无人机以不同的任务，将无人机各自的扫掠结果统一传回终端，在终端进行综合分析，这样就可以加快地图探索。\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 5. 总结与收获\n",
    "\n",
    "本次课设我选择使用Matlab实现，原因之一是对于Matlab比较熟悉，其次就是Matlab的可视化效果要好一点，画图简单，读取文件和debug也要方便许多。虽然没有使用Qt和C++实现，但是算法原理以及搞清楚了，用其他语言也应该能够实现。\n",
    "    \n",
    "首先最大的收获就是学习和使用A* 算法。由于很少人用Matlab实现，所以在学习了理论知识之后，没有可以借鉴的代码，因此就只能自己按照步骤一步一步实现，这个过程也是我感觉收获最多的过程，一步一步实现，一点一点的改错，不仅让我对算法的整个过程，甚至是一些小细节都熟记于心，更锻炼了我的编程能力。\n",
    "\n",
    "其次是让我更熟练的使用和学习了matlab的动态画图操作以及网格图的绘制，文件的读取以及数据的处理。整个课设实现下来，会产生许多有用的数据，如何将这些数据保存下来，并且合理的安排数据结构，才能在整个过程中不会搞糊涂，有一些重要的数据还要确保代码不会把这些数据修改和覆盖。\n",
    "\n",
    "在动态路径规划的实现过程中，没有使用什么算法，大部分是自己的思考，因此在实现想法和想法可行性之间总有权衡，有时候会有想到了没法实现，或者是实现了但效果很差的情况，我也经历过几次这样的过程，写费了很多代码，最终也就在感觉差不多，且实现步骤相对简单的想法，虽然比较费时，但也锻炼了自己的思维。"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.8"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
